package com.gitee.wsl.mathematics.complex.quaternion.ext


/*


/**
 * Construct [Quaternion] from axes
 */
fun fromAxes(x: Vector3, y: Vector3, z: Vector3): Quaternion {
    val m = Matrix33.fromColumnVectors(x, y, z)
    return fromMatrix(m)
}

fun<T:Number> Quaternion<T, *>.set(m:Matrix3<T, *, *> ){
    val t = m.trace + 1.0
    val x: Double
    val y: Double
    val z: Double
    val w: Double
    if (t > 0) {
        val s = 0.5 / sqrt(t)
        w = 0.25 / s
        x = (m.c1r2 - m.c2r1) * s
        y = (m.c2r0 - m.c0r2) * s
        z = (m.c0r1 - m.c1r0) * s
    } else if (m.c0r0 > m.c1r1 && m.c0r0 > m.c2r2) {
        val s = 0.5 / sqrt(1.0 + m.c0r0 - m.c1r1 - m.c2r2) // S=4*qx
        w = (m.c1r2 - m.c2r1) * s
        x = 0.25f / s
        y = (m.c0r1 + m.c1r0) * s
        z = (m.c2r0 + m.c0r2) * s
    } else if (m.c1r1 > m.c2r2) {
        val s = 0.5f / sqrt(1.0 + m.c1r1 - m.c0r0 - m.c2r2) // S=4*qy
        w = (m.c2r0 - m.c0r2) * s
        x = (m.c0r2 + m.c1r0) * s
        y = 0.25f / s
        z = (m.c1r2 + m.c2r1) * s
    } else {
        val s = 0.5f / sqrt(1.0 + m.c2r2 - m.c0r0 - m.c1r1) // S=4*qz
        w = (m.c0r1 - m.c1r0) * s
        x = (m.c2r0 + m.c0r2) * s
        y = (m.c1r2 + m.c2r1) * s
        z = 0.25f / s
    }
    return set(x, y, z, w)

}
fun fromVectors(from: Vector3F, to: Vector3F): Quaternion {
            // Normalize input vectors
            val start = from.normalized()
            val dest = to.normalized()

            val dot = start.dot(dest)

            // If vectors are opposite
            when {
                dot < -0.9999999f -> {
                    val tmp = Vector3F(start.y, -start.x, 0f).normalized()
                    return Quaternion(tmp.x, tmp.y, tmp.z, 0f)
                }

                dot > 0.9999999f -> {
                    // If vectors are same
                    return Quaternion()
                }

                else -> {
                    val s = kotlin.math.sqrt((1 + dot) * 2)
                    val invs = 1 / s

                    val c = start.cross(dest)

                    return Quaternion(
                        c.x * invs,
                        c.y * invs,
                        c.z * invs,
                        s * 0.5f,
                    ).normalized()
                }
            }
        }

        fun fromAxisAngle(axis: Vector3F, angle: Angle): Quaternion {
            val naxis = axis.normalized()
            val angle2 = angle / 2
            val s = sin(angle2)
            return Quaternion(
                naxis.x * s,
                naxis.y * s,
                naxis.z * s,
                cos(angle2)
            )
        }

        // @TODO: Check
        fun lookRotation(forward: Vector3F, up: Vector3F = Vector3F.UP): Quaternion {
            //if (up == Vector3.UP) return fromVectors(Vector3.FORWARD, forward.normalized())
            val z = forward.normalized()
            val x = (up.normalized() cross z).normalized()

            //println("x=$x, z=$z")
            if (x.lengthSquared.isAlmostZero()) {
                // COLLINEAR
                return Quaternion.fromVectors(Vector3F.FORWARD, z)
            }

            val y = z cross x
            return fromRotationMatrix(Matrix3.fromColumns(x, y, z))
        }


    /** Sets the quaternion components from the given axis and angle in radians around that axis. */
    fun setQuaternionByAxis(axis: IVec3, radians: Float): IVec4 {
        return setQuaternionByAxis(axis.x, axis.y, axis.z, radians)
    }

    /** Sets the quaternion components from the given axis and angle around that axis.
     * @param x X direction of the axis
     * @param y Y direction of the axis
     * @param z Z direction of the axis
     * @param radians The angle in radians
     * @return This quaternion for chaining.
     */
    fun setQuaternionByAxis(x: Float, y: Float, z: Float, radians: Float): IVec4 {
        var d = MATH.len2(x, y, z)
        if (d == 0f) return idtQuaternion()
        d = 1f / MATH.sqrt(d)
        val ang = if (radians < 0) MATH.PI2 - -radians % MATH.PI2 else radians % MATH.PI2
        val sin = MATH.sin(ang * 0.5f)
        val cos = MATH.cos(ang * 0.5f)
        return set(d * x * sin, d * y * sin, d * z * sin, cos).nor()
    }


*/


